import java.io.BufferedReader;
import java.io.File;
import java.io.FileNotFoundException;
import java.io.FileReader;
import java.io.InputStreamReader;
import java.io.OutputStreamWriter;
import java.lang.reflect.Type;
import java.net.HttpURLConnection;
import java.net.URL;
import java.net.URLConnection;
import java.util.LinkedList;
import java.util.Map;

import com.google.gson.Gson;
import com.google.gson.JsonIOException;
import com.google.gson.JsonSyntaxException;
import com.google.gson.reflect.TypeToken;

public class HorseSimulator9k {

    private static HorseSimulator9k robot;
    private final Gson gson;
    private final LocalizationResponse lr = new LocalizationResponse();
    private final DifferentialDriveRequest dr = new DifferentialDriveRequest();
    private String host = "http://127.0.0.1";
    private int port = 50000;
    private static File pathFile;
    final LinkedList<PoseData> path;
    double lookAhead = 0.6;

    public static void main(final String[] args) throws Exception {
	if (args.length < 1) {
	    System.out.println("Error: Need to specify path file.");
	    System.exit(1);
	}
	pathFile = new File(args[0]);

	if (!pathFile.canRead()) {
	    System.out.println("Error: Path file could not be read.");
	    System.exit(1);
	}
	new HorseSimulator9k();

    }

    /**
     * The constructor for the robot that follows the given path using the
     * "Follow the carrot" method.
     * 
     */
    public HorseSimulator9k() throws JsonIOException, JsonSyntaxException,
	    FileNotFoundException {

	robot = this;
	gson = new Gson();

	final Type collectionType = new TypeToken<LinkedList<PoseData>>() {
	}.getType();
	path = gson.fromJson(new FileReader(pathFile), collectionType);
	try {
	    move();
	} catch (Exception e) {
	    e.printStackTrace();
	}

    }

    /**
     * Moves the robot along the path. The path tracking algorithm is
     * "Follow the carrot".
     * 
     */
    private void move() throws Exception {
	Position pathPos;
	Position robotPos;
	double linearSpeed;
	double angularSpeed;
	double headingAngle;
	double errorAngle;
	double bearingToPathPos;

	for (int i = 0; i < path.size(); i++) {

	    robot.getResponse(lr);
	    robotPos = new Position(lr.getPosition()[0], lr.getPosition()[1]);
	    headingAngle = lr.getHeadingAngle();

	    pathPos = path.get(i).getPose().getPosition();
	    double distToPathPos = robotPos.getDistanceTo(pathPos);
	    /*
	     * Skip positions on the path that are closer then lookAhead
	     * distance.
	     */
	    if (distToPathPos < lookAhead) {
		continue;
	    }
	    bearingToPathPos = robotPos.getBearingTo(pathPos);

	    /*
	     * Drive the robot towards the position until the distance is at
	     * least 0.3 Units of length away from it.
	     */
	    do {
		robot.getResponse(lr);
		robotPos = new Position(lr.getPosition()[0],
			lr.getPosition()[1]);
		headingAngle = lr.getHeadingAngle();
		errorAngle = headingAngle - bearingToPathPos;

		/*
		 * Calculate the optimal(shortest) way(left/right) for the robot
		 * to turn towards the goal.
		 */
		if (Math.toDegrees(errorAngle) > 180
			|| Math.toDegrees(errorAngle) < -180) {
		    errorAngle = -1 * errorAngle;
		}

		distToPathPos = robotPos.getDistanceTo(pathPos);

		linearSpeed = adjustLinearSpeed(errorAngle);
		dr.setLinearSpeed(linearSpeed);
		angularSpeed = adjustAngularSpeed(errorAngle);
		dr.setAngularSpeed(angularSpeed);

		robot.putRequest(dr);

	    } while (distToPathPos >= 0.3);

	    dr.setLinearSpeed(0);
	    dr.setAngularSpeed(0);
	    robot.putRequest(dr);
	}

    }

    /**
     * Returns an appropriate angular speed given the angle to the robots goal.
     * 
     * @param errorAngle
     *            the angle in radians from the robot to its goal.
     * @return an angular speed appropriate with respect to the angle.
     */
    private double adjustAngularSpeed(double errorAngle) {
	double angleDeg = Math.abs(Math.toDegrees(errorAngle));
	double speedconst;

	if (angleDeg < 5) {
	    speedconst = -1;
	} else if (angleDeg < 10 && angleDeg >= 5) {
	    speedconst = -3;
	} else if (angleDeg < 20 && angleDeg >= 10) {
	    speedconst = -5;
	} else {
	    speedconst = -6;
	}

	return speedconst * errorAngle;
    }

    /**
     * Returns an appropriate linear speed given the angle to the robots goal.
     * 
     * @param errorAngle
     *            the angle in radians from the robot to its goal.
     * @return a linear speed appropriate with respect to the angle.
     */
    private double adjustLinearSpeed(double errorAngle) {
	double angleDeg = Math.abs(Math.toDegrees(errorAngle));
	double linSpeed;
	if (angleDeg < 5) {
	    linSpeed = 1;
	} else if (angleDeg < 10 && angleDeg >= 5) {
	    linSpeed = 0.8;
	} else if (angleDeg < 40 && angleDeg >= 10) {
	    linSpeed = 0.6;
	} else if (angleDeg < 90 && angleDeg >= 40) {
	    linSpeed = 0.4;
	} else if (angleDeg >= 90 && angleDeg < 120) {
	    linSpeed = 0.3;
	} else {
	    linSpeed = 0;
	}
	return linSpeed;
    }

    /**
     * Send a request to the robot.
     * 
     * @param r
     *            request to send
     * @return response code from the connection (the web server)
     * @throws Exception
     */
    public int putRequest(final Request r) throws Exception {
	final URL url = new URL(host + ":" + port + r.getPath());

	final HttpURLConnection connection = (HttpURLConnection) url
		.openConnection();

	connection.setDoOutput(true);

	connection.setRequestMethod("POST");
	connection.setRequestProperty("Content-Type", "application/json");
	connection.setUseCaches(false);

	final OutputStreamWriter out = new OutputStreamWriter(
		connection.getOutputStream());

	// construct a JSON string
	final String json = gson.toJson(r.getData());

	// write it to the web server
	out.write(json);
	out.close();

	// wait for response code
	final int rc = connection.getResponseCode();

	return rc;
    }

    /**
     * Get a response from the robot
     * 
     * @param r
     *            response to fill in
     * @return response same as parameter
     * @throws Exception
     */
    public Response getResponse(final Response r) throws Exception {

	final URL url = new URL(host + ":" + port + r.getPath());

	// open a connection to the web server and then get the resulting data
	final URLConnection connection = url.openConnection();
	final BufferedReader in = new BufferedReader(new InputStreamReader(
		connection.getInputStream()));

	final Type sObjMap = new TypeToken<Map<String, Object>>() {
	}.getType();
	final Map<String, Object> data = gson.fromJson(in, sObjMap);

	r.setData(data);

	in.close();

	return r;
    }
}
